﻿#include "OiPCH.hpp"
#include "Proc/OiFitLine.hpp"

namespace Oi
{
    namespace PLScannerCore
    {
        class WideCalibTask : public QThread
        {
        public:
            void run();
            void showNGMessage(QString msg);
            void showOKMessage(QString msg);

        public:
            bool _quit;
            WideCalibDlg* _dlg;
            OiProfile _output;
            bool _calibDone;
        };

        void WideCalibTask::run()
        {
            _quit = false;
            _calibDone = false;
            _output.clear();

            showOKMessage(WideCalibDlg::tr("正在标定..."));

            if (PLScanner::instance()->simulation)
            {
                QThread::msleep(1000);
                qsrand(QTime(0, 0, 0).secsTo(QTime::currentTime()));

                for (int i = 0; i < 201; i++)
                {
                    _output.append(QPointF(i - 100.0,  3.0 + 0.0001 * (qrand() % 1000) - 0.05));
                }

                _calibDone = true;
                showOKMessage(WideCalibDlg::tr("标定成功"));

                return;
            }

            Motor::instance()->locateMode();

            Motor::instance()->locatedRun(0.0);

            showOKMessage(WideCalibDlg::tr("正在等待轴运动至起始点"));

            QThread::msleep(1000);

            int timerCounter = 0;

            while (!Motor::instance()->locatedModeReady())
            {
                if (_quit)
                {
                    return;
                }

                if (timerCounter > 300)
                {
                    showNGMessage(WideCalibDlg::tr("等待轴运动过程超时"));

                    return;
                }

                timerCounter++;

                QThread::msleep(100);
            }

            showOKMessage(WideCalibDlg::tr("正在标定..."));

            Motor::instance()->loopMode();

            OiProfile profile;

            bool dir = true;

            int timeout = PLScanner::instance()->captureTimeout * 1000;
            
            if (!SensorAcquire::OiAcquire()->acquire(nullptr, nullptr, profile, dir, PLScanner::instance()->acquireVel, 0.0, 0.0, true, timeout))
            {
                showNGMessage(WideCalibDlg::tr("采集数据超时"));

                return;
            }

            if (_quit)
            {
                return;
            }

            for (auto& p : profile)
            {
                p.setY(p.y() - PLScanner::instance()->measureOffsetY - PLScanner::instance()->measureOffsetY_S);
            }

            OiProfile dataProfile = profile;

            if (profile.size() >= 7000)
            {
                PLScannerUtil::filter(dataProfile, PLScanner::instance()->sideCorrect, 0.5);

                if (dataProfile.size() < 7000)
                {
                    showNGMessage(WideCalibDlg::tr("标定失败，未采集到标定块"));
                    return;
                }

                _output = profile;

                _calibDone = true;
            }
            else
            {
                showNGMessage(WideCalibDlg::tr("标定失败，数据量不够"));
            }
        }

        void WideCalibTask::showNGMessage(QString msg)
        {
            if (_dlg)
            {
                QMetaObject::invokeMethod(_dlg, "showErrStatusMsg", Qt::AutoConnection, Q_ARG(QString, msg));
            }
        }

        void WideCalibTask::showOKMessage(QString msg)
        {
            if (_dlg)
            {
                QMetaObject::invokeMethod(_dlg, "showOKStatusMsg", Qt::AutoConnection, Q_ARG(QString, msg));
            }
        }

        class WideCalibDlgImpl : public Impl<WideCalibDlg>
        {
        public:
            void init();
            void exit();

        public:
            QLineEdit* _actual;
            QPushButton* _calib;
            LineView* _view;

        public:
            QList<double> _values;
            QLabel* _msg;
            WideCalibTask _task;
        };

        void WideCalibDlgImpl::init()
        {
            OI_Q(WideCalibDlg);

            q->setWindowTitle(CalibDlg::tr("连续标定"));

            QGridLayout* layout = new QGridLayout(q);

            QLabel* al = new QLabel("实际值");
          
            _actual = new QLineEdit("0.00");
            _actual->setValidator(new QDoubleValidator());

            _actual->setText(QString::number(Core::OiSettings()->value("Calib/ActuralValue2", 0.0).toDouble(), 'f', 2));

            _calib = new QPushButton("开始标定");

            _view = new LineView();

            _msg = new QLabel();
            _msg->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
          
            layout->addWidget(al, 0, 0);
            layout->addWidget(_actual, 0, 1);
            layout->addWidget(_calib, 1, 0);
            layout->addWidget(_view, 2, 0, 4, 2);
            layout->addWidget(_msg, 6, 1);

            _task._dlg = q;

            q->connect(_calib, &QPushButton::clicked, q, &WideCalibDlg::onCalib);
            q->connect(&_task, &QThread::finished, q, &WideCalibDlg::onCalibDone);
        }

        void WideCalibDlgImpl::exit()
        {
            SensorAcquire::OiAcquire()->abort();
            _task._quit = true;
            _task.wait();
        }

        WideCalibDlg::WideCalibDlg(QWidget* parent /*= nullptr*/)
        {
            OI_I(WideCalibDlg)->init();
        }

        WideCalibDlg::~WideCalibDlg()
        {
            OI_E(WideCalibDlg)->exit();
        }

        void WideCalibDlg::onCalib()
        {
            OI_F(WideCalibDlg);

            bool convert = false;
            double actualValue = d->_actual->text().toDouble(&convert);

            Core::OiSettings()->setValue("Calib/ActuralValue2", actualValue);
            Core::OiSettings()->sync();

            if (!convert)
            {
                showErrStatusMsg(tr("实际值有错误，标定不成功"));
                return;
            }

            if (d->_task.isRunning())
            {
                showErrStatusMsg(tr("任务正在执行，标定不成功"));
                return;
            }

            d->_actual->setEnabled(false);
            d->_calib->setEnabled(false);

            Environment::_startTask = true;

            d->_task.start();
        }

        void WideCalibDlg::onCalibDone()
        {
            OI_F(WideCalibDlg);

            d->_actual->setEnabled(true);
            d->_calib->setEnabled(true);


            if (d->_task._calibDone)
            {
                d->_view->display(d->_task._output);

                auto actualValue = Core::OiSettings()->value("Calib/ActuralValue2", 0.0).toDouble();

                auto ps = d->_task._output;

//                PLScannerUtil::filter(ps, 0.3);

                for (auto& p : ps)
                {
                    p.setY(actualValue - p.y());
                }
                int pss = ps.size();

                for (int i = 0; i < pss; i++)
                {
                    int tcount = 0;
                    double tvalue = 0.0;
                    for (int j = qMax(i - 10, 0); j <= qMin(i+10, pss-1); j++)
                    {
                        tvalue += ps[j].y();
                        tcount++;
                    }

                    ps[i].setY(tvalue / tcount);
                }

                QByteArray pBA;
                QDataStream pDS(&pBA, QIODevice::WriteOnly);
                pDS << ps;
            
                Core::OiSettings()->setValue("Calib/WideCalib-P", pBA);
                Core::OiSettings()->sync();

                PLScanner::instance()->wideCalib_P = ps;

                showOKStatusMsg(tr("标定成功"));

//                 Base::List<Geom::Point2d> fitPoints;
//                 Geom::Line2d fitSideLine(CGAL::ORIGIN, CGAL::ORIGIN);
// 
//                 for (auto p : ps)
//                 {
//                     fitPoints.push_back(Geom::Point2d(p.x(), p.y()));
//                 }
// 
//                 Proc::FitLineD::execute(fitPoints, fitSideLine);
// 
//                 if (fitSideLine.is_degenerate())
//                 {
//                     showErrStatusMsg(tr("数据不完整，失败"));
//                 }
//                 else
//                 {
//                     double a = -fitSideLine.a() / fitSideLine.b();
//                     double b = -fitSideLine.c() / fitSideLine.b();
// 
//                     Core::OiSettings()->setValue("Calib/WideCalib-A", a);
//                     Core::OiSettings()->setValue("Calib/WideCalib-B", b);
//                     Core::OiSettings()->sync();
// 
//                     showOKStatusMsg(tr("标定成功"));
//                 }
            }

            Environment::_startTask = false;
        }

        void WideCalibDlg::showOKStatusMsg(QString msg)
        {
            OI_F(WideCalibDlg);

            d->_msg->setText(QString("<font color = \"#008000\">%1</font>").arg(msg));
        }

        void WideCalibDlg::showErrStatusMsg(QString msg)
        {
            OI_F(WideCalibDlg);
           
            d->_msg->setText(QString("<font color = \"#ff0000\">%1</font>").arg(msg));
        }

    }
}